#include "Rt_LUT_Config.h"

void Rt_LUT_Config(XRt1553B *InstancePtr)
{
    u32 foo = 0;
    u32 tmp = 0;
    // 接收子地址配置
    for (u32 i = 0; i < 32; i++)
    {
        switch (InstancePtr->rx_SAcfg[i].mm_mode)
        {
        case XRT53_MM_SINGLE_MSG:                                                  // 单消息模式
            foo = XRT53_DATABLOCK_ADDR(InstancePtr->rx_SAcfg[i].DataBlock_number); // 写接收消息指针查询表
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_RX(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_RX(i), foo);
            InstancePtr->rx_SAcfg[i].DataBlock_start_ptr = foo;

            foo = XRt1553B_ReadMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i)); // 写子地址控制字
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_DOUBLE_BUFFER_ENABLE);
            XRt1553B_SET_BIT(foo, XRT53_SACW_RX_EOM_INTR);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_RX_CIRC_BUF_INTR);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_RX_MM0);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_RX_MM1);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_RX_MM2);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_SACW(i), foo);
            break;

        case XRT53_MM_DOUBLE_BUFFER:                                               // 双缓冲模式
            foo = XRT53_DATABLOCK_ADDR(InstancePtr->rx_SAcfg[i].DataBlock_number); // 写接收消息指针查询表
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_RX(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_RX(i), foo);
            InstancePtr->rx_SAcfg[i].DataBlock_start_ptr = foo;

            foo = XRt1553B_ReadMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i)); // 写子地址控制字
            XRt1553B_SET_BIT(foo, XRT53_SACW_DOUBLE_BUFFER_ENABLE);
            XRt1553B_SET_BIT(foo, XRT53_SACW_RX_EOM_INTR);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_RX_CIRC_BUF_INTR);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_RX_MM0);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_RX_MM1);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_RX_MM2);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_SACW(i), foo);
            break;

        case XRT53_MM_CIRCULAR_BUFFER:                          // 循环缓冲模式
            foo = InstancePtr->rx_SAcfg[i].DataBlock_start_ptr; // 写接收消息指针查询表 用户自己算
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_RX(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_RX(i), foo);

            foo = XRt1553B_ReadMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i)); // 写子地址控制字
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_DOUBLE_BUFFER_ENABLE);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_RX_EOM_INTR);
            XRt1553B_SET_BIT(foo, XRT53_SACW_RX_CIRC_BUF_INTR);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_RX_MM0);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_RX_MM1);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_RX_MM2);

            tmp = datalenth_2_MM_mode(InstancePtr->rx_SAcfg[i].valid_data_bytes); // 计算mm0~mm2
            foo = foo | (((tmp >> 0) & 0x1) << XRT53_SACW_RX_MM0);
            foo = foo | (((tmp >> 1) & 0x1) << XRT53_SACW_RX_MM1);
            foo = foo | (((tmp >> 2) & 0x1) << XRT53_SACW_RX_MM2);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_SACW(i), foo);
            break;
        default: // XRT53_MM_UNUSE模式和其他 指向未使用的子地址
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_RX(i), XRT53_DATABLOCK_UNUSED);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_RX(i), XRT53_DATABLOCK_UNUSED);
            foo = XRt1553B_ReadMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i));
            foo &= XRT53_SACW_RX_UNUSED_MASK; // 禁止中断 单消息
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_SACW(i), foo);
            break;
        }
    }
    // 接收子地址查找表配置完成

    // 发送子地址配置
    for (u32 i = 0; i < 32; i++)
    {
        switch (InstancePtr->tx_SAcfg[i].mm_mode)
        {
        case XRT53_MM_SINGLE_MSG:                                                  // 单消息模式
            foo = XRT53_DATABLOCK_ADDR(InstancePtr->tx_SAcfg[i].DataBlock_number); // 写接收消息指针查询表
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_TX(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_TX(i), foo);
            InstancePtr->tx_SAcfg[i].DataBlock_start_ptr = foo;

            foo = XRt1553B_ReadMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i)); // 写子地址控制字
            XRt1553B_SET_BIT(foo, XRT53_SACW_TX_EOM_INTR);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_TX_CIRC_BUF_INTR);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_TX_MM0);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_TX_MM1);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_TX_MM2);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_SACW(i), foo);
            break;

        case XRT53_MM_CIRCULAR_BUFFER:                          // 循环缓冲模式
            foo = InstancePtr->tx_SAcfg[i].DataBlock_start_ptr; // 写接收消息指针查询表 用户自己算
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_TX(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_TX(i), foo);

            foo = XRt1553B_ReadMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i)); // 写子地址控制字
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_TX_EOM_INTR);
            XRt1553B_SET_BIT(foo, XRT53_SACW_TX_CIRC_BUF_INTR);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_TX_MM0);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_TX_MM1);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_TX_MM2);

            tmp = datalenth_2_MM_mode(InstancePtr->tx_SAcfg[i].valid_data_bytes); // 计算mm0~mm2
            foo = foo | (((tmp >> 0) & 0x1) << XRT53_SACW_TX_MM0);
            foo = foo | (((tmp >> 1) & 0x1) << XRT53_SACW_TX_MM1);
            foo = foo | (((tmp >> 2) & 0x1) << XRT53_SACW_TX_MM2);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_SACW(i), foo);
            break;
        default: // XRT53_MM_UNUSE和XRT53_MM_DOUBLE_BUFFER等 指向未使用的子地址
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_TX(i), XRT53_DATABLOCK_UNUSED);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_TX(i), XRT53_DATABLOCK_UNUSED);
            foo = XRt1553B_ReadMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i));
            foo &= XRT53_SACW_TX_UNUSED_MASK; // 禁止中断 单消息
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_SACW(i), foo);
            break;
        }
    }
    // 发送子地址查找表配置完成

    // 广播子地址配置
    for (u32 i = 0; i < 32; i++)
    {
        switch (InstancePtr->bcst_SAcfg[i].mm_mode)
        {
        case XRT53_MM_SINGLE_MSG:                                                    // 单消息模式
            foo = XRT53_DATABLOCK_ADDR(InstancePtr->bcst_SAcfg[i].DataBlock_number); // 写接收消息指针查询表
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_BCST(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_BCST(i), foo);
            InstancePtr->bcst_SAcfg[i].DataBlock_start_ptr = foo;

            foo = XRt1553B_ReadMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i)); // 写子地址控制字
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_DOUBLE_BUFFER_ENABLE);
            XRt1553B_SET_BIT(foo, XRT53_SACW_BCST_EOM_INTR);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_BCST_CIRC_BUF_INTR);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_BCST_MM0);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_BCST_MM1);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_BCST_MM2);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_SACW(i), foo);
            break;

        case XRT53_MM_DOUBLE_BUFFER:                                                 // 双缓冲模式
            foo = XRT53_DATABLOCK_ADDR(InstancePtr->bcst_SAcfg[i].DataBlock_number); // 写接收消息指针查询表
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_BCST(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_BCST(i), foo);
            InstancePtr->bcst_SAcfg[i].DataBlock_start_ptr = foo;

            foo = XRt1553B_ReadMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i)); // 写子地址控制字
            XRt1553B_SET_BIT(foo, XRT53_SACW_DOUBLE_BUFFER_ENABLE);
            XRt1553B_SET_BIT(foo, XRT53_SACW_BCST_EOM_INTR);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_BCST_CIRC_BUF_INTR);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_BCST_MM0);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_BCST_MM1);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_BCST_MM2);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_SACW(i), foo);
            break;

        case XRT53_MM_CIRCULAR_BUFFER:                            // 循环缓冲模式
            foo = InstancePtr->bcst_SAcfg[i].DataBlock_start_ptr; // 写接收消息指针查询表 用户自己算
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_BCST(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_BCST(i), foo);

            foo = XRt1553B_ReadMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i)); // 写子地址控制字
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_DOUBLE_BUFFER_ENABLE);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_BCST_EOM_INTR);
            XRt1553B_SET_BIT(foo, XRT53_SACW_BCST_CIRC_BUF_INTR);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_BCST_MM0);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_BCST_MM1);
            XRt1553B_CLEAR_BIT(foo, XRT53_SACW_BCST_MM2);

            tmp = datalenth_2_MM_mode(InstancePtr->bcst_SAcfg[i].valid_data_bytes); // 计算mm0~mm2
            foo = foo | (((tmp >> 0) & 0x1) << XRT53_SACW_BCST_MM0);
            foo = foo | (((tmp >> 1) & 0x1) << XRT53_SACW_BCST_MM1);
            foo = foo | (((tmp >> 2) & 0x1) << XRT53_SACW_BCST_MM2);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_SACW(i), foo);
            break;
        default: // XRT53_MM_UNUSE模式和其他 指向未使用的子地址
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_BCST(i), XRT53_DATABLOCK_UNUSED);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_BCST(i), XRT53_DATABLOCK_UNUSED);
            foo = XRt1553B_ReadMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i));
            foo &= XRT53_SACW_BCST_UNUSED_MASK; // 禁止中断 单消息
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_SACW(i), foo);
            XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_SACW(i), foo);
            break;
        }
    }
    // 广播子地址查找表配置完成
    return;
}

// 向发送子地址写数据函数 以字为单位
// 返回值为实际发送的数据 pdata存放数据
u32 XRt1553B_TxToSubAddr(XRt1553B *InstancePtr, u32 SubAddr, const u8 *pdata, u32 ByteLenth)
{
    u32 cnt = 0; // 发送计数 用于返回值
    u32 foo = 0;
    //    u16 tmp = 0;
    u32 WordLenth = 0;
    u16 *ptr = (u16 *)pdata;

    if (pdata == NULL)
    {
        return 0;
    }
    
    // 判断读取字节的有效性 当形参中长度大于配置的长度 则返回0
    foo = InstancePtr->tx_SAcfg[SubAddr].valid_data_bytes;
    if (foo < ByteLenth)
    {
        return 0;
    }

    if (0 == (ByteLenth % 2)) // 偶数字节
    {
        WordLenth = ByteLenth >> 1;
    }
    else // 奇数字节 丢弃最后一个字节避免数组越界
    {
        WordLenth = (ByteLenth - 1) >> 1;
    }

    // 获取指针 并保存数据
    foo = InstancePtr->tx_SAcfg[SubAddr].DataBlock_start_ptr;
    for (u32 i = 0; i < WordLenth; i++)
    {
        // tmp = Xil_Htons(ptr[i]);
        // XRt1553B_WriteMem(InstancePtr->BaseAddress, (foo + i), tmp);
        XRt1553B_WriteMem(InstancePtr->BaseAddress, (foo + i), ptr[i]);
        cnt += 2;
    }

    // 循环缓冲 则拉回指针
    if (XRT53_MM_CIRCULAR_BUFFER == InstancePtr->tx_SAcfg[SubAddr].mm_mode)
    {
        foo = InstancePtr->tx_SAcfg[SubAddr].DataBlock_start_ptr;
        XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_TX(SubAddr), foo);
        XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_TX(SubAddr), foo);
    }

    return cnt;
}


u32 XRt1553B_BCSTFromSubAddr(XRt1553B *InstancePtr, u32 SubAddr, u8 *pdata, u32 ByteLenth)
{
    u32 cnt = 0; // 接收计数 用于返回值
    u32 foo = 0;
    u32 WordLenth = 0;
    u16 *ptr = (u16 *)pdata;

    // 判断读取字节的有效性 当形参中长度大于配置的长度 则返回0
    foo = InstancePtr->bcst_SAcfg[SubAddr].valid_data_bytes;
    if (foo < ByteLenth)
    {
        return 0;
    }

    if (0 == (ByteLenth % 2)) // 偶数字节
    {
        WordLenth = ByteLenth >> 1;
    }
    else // 奇数字节 丢弃最后一个字节避免数组越界
    {
        WordLenth = (ByteLenth - 1) >> 1;
    }

    // 获取指针 并保存数据
    foo = InstancePtr->bcst_SAcfg[SubAddr].DataBlock_start_ptr;
    for (u32 i = 0; i < WordLenth; i++)
    {
        ptr[i] = (u16)XRt1553B_ReadMem(InstancePtr->BaseAddress, foo + i);
        cnt += 2;
    }

    // 循环缓冲 则拉回指针
    if (XRT53_MM_CIRCULAR_BUFFER == InstancePtr->bcst_SAcfg[SubAddr].mm_mode)
    {
        foo = InstancePtr->bcst_SAcfg[SubAddr].DataBlock_start_ptr;
        XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_BCST(SubAddr), foo);
        XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_BCST(SubAddr), foo);
    }

    return cnt;
}


u32 XRt1553B_RxFromSubAddr(XRt1553B *InstancePtr, u32 SubAddr, u8 *pdata, u32 ByteLenth)
{
    u32 cnt = 0; // 接收计数 用于返回值
    u32 foo = 0;
    u32 WordLenth = 0;
    u16 *ptr = (u16 *)pdata;

    // 判断读取字节的有效性 当形参中字节长度大于配置的长度 则返回0
    foo = InstancePtr->rx_SAcfg[SubAddr].valid_data_bytes;
    if (foo < ByteLenth)
    {
        return 0;
    }

    if (0 == (ByteLenth % 2)) // 偶数字节
    {
        WordLenth = ByteLenth >> 1;
    }
    else // 奇数字节 丢弃最后一个字节避免数组越界
    {
        WordLenth = (ByteLenth - 1) >> 1;
    }

    // 获取指针 并保存数据
    foo = InstancePtr->rx_SAcfg[SubAddr].DataBlock_start_ptr;
    for (u32 i = 0; i < WordLenth; i++)
    {
        ptr[i] = (u16)XRt1553B_ReadMem(InstancePtr->BaseAddress, foo + i);
        cnt += 2;
    }

    // 循环缓冲 则拉回指针
    if (XRT53_MM_CIRCULAR_BUFFER == InstancePtr->rx_SAcfg[SubAddr].mm_mode)
    {
        foo = InstancePtr->rx_SAcfg[SubAddr].DataBlock_start_ptr;
        XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_A_RX(SubAddr), foo);
        XRt1553B_WriteMem(InstancePtr->BaseAddress, XRT53_LUT_B_RX(SubAddr), foo);
    }

    return cnt;
}



u32 XRt1553B_GetCMDInfo(XRt1553B *InstancePtr, XRt1553B_MsgBlkDesc *CMDInfo)
{
    u32 foo = 0;
    u32 stack_writePtr = 0; // 写指针 即MEM0x100地址的指针

    foo = XRt1553B_ReadReg(InstancePtr->BaseAddress, XRT53_CFG1_OFFSET);
    if ((foo & XRT53_CFG1_MSG_BUSY) > 0) // 正在处理消息 函数返回
    {
        return 0;
    }

    // 读取堆栈指针 如果一致则表示没有新消息
    stack_writePtr = XRt1553B_ReadMem(InstancePtr->BaseAddress, XRT53_STACKA_PTR_OFFSET);
    if (stack_writePtr == InstancePtr->CMDStack_ReadPtr)
    {
        return 0;
    }

    // 有新消息 根据读指针读取消息
    foo = (InstancePtr->CMDStack_ReadPtr) & 0xFF; // 地址保护
    CMDInfo->BlockStatusWord = XRt1553B_ReadMem(InstancePtr->BaseAddress, foo);
    CMDInfo->TimeTag = XRt1553B_ReadMem(InstancePtr->BaseAddress, foo + 1);
    CMDInfo->DataBlkPtr_or_ModeWord = XRt1553B_ReadMem(InstancePtr->BaseAddress, foo + 2);
    CMDInfo->CMDWord = XRt1553B_ReadMem(InstancePtr->BaseAddress, foo + 3);

    // 消息开始 则不处理
    if ((XRT53_BLOCK_STATWORDS_SOM_MASK & CMDInfo->BlockStatusWord) > 0)
    {
        return 0;
    }

    CMDInfo->BUSA_or_BUSB = 1 & ((CMDInfo->BlockStatusWord) >> XRT53_BLOCK_STATWORDS_BUSAB_LOC);
    CMDInfo->CircBuff_rollover = 1 & ((CMDInfo->BlockStatusWord) >> XRT53_BLOCK_STATWORDS_ROLLOVER_LOC);
    CMDInfo->blk_err_stat = XRT53_BLOCK_STATWORDS_ERR_MASK & (CMDInfo->BlockStatusWord);

    CMDInfo->RTaddr = 0x1F & (CMDInfo->CMDWord >> XRT53_CMD_WORD_RTADDR_LOC);
    CMDInfo->T_or_R = 1 & (CMDInfo->CMDWord >> XRT53_CMD_WORD_T_OR_R_LOC);
    CMDInfo->SubAddr_or_ModeCMD = 0x1F & (CMDInfo->CMDWord >> XRT53_CMD_WORD_SADDR_OR_MODE_LOC);
    CMDInfo->DataLen_or_ModeCode = 0x1F & (CMDInfo->CMDWord);

    // 处理完成 读地址+4 并保护不越界
    InstancePtr->CMDStack_ReadPtr = (InstancePtr->CMDStack_ReadPtr + 4) % 256;

    if (stack_writePtr >= InstancePtr->CMDStack_ReadPtr) // 写指针没有到256
    {
        foo = stack_writePtr - InstancePtr->CMDStack_ReadPtr;
    }
    else // 写指针翻转的处理 用于多个消息未处理的翻转
    {
        foo = stack_writePtr + 256 - InstancePtr->CMDStack_ReadPtr;
    }

    foo = 1 + (foo >> 2); // 除以4后加1 表示待处理的指令数量
    return foo;
}